#ifndef DWA_PLANNER_HPP
#define DWA_PLANNER_HPP

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <vector>
#include <cmath>

namespace auto_navigation {

struct Trajectory {
    std::vector<geometry_msgs::PoseStamped> poses;
    double v;  // 线速度
    double w;  // 角速度
    double score;
};

class DWAPlanner {
public:
    DWAPlanner();
    ~DWAPlanner();

    // 初始化规划器
    bool initialize(ros::NodeHandle& nh);
    
    // 更新全局路径
    void updatePlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
    
    // 计算控制命令
    geometry_msgs::Twist computeVelocityCommands(const geometry_msgs::PoseStamped& current_pose,
                                                const geometry_msgs::Twist& current_velocity);

private:
    // 生成可能的轨迹
    std::vector<Trajectory> generateTrajectories(const geometry_msgs::Twist& current_velocity);
    
    // 评估轨迹
    double evaluateTrajectory(const Trajectory& traj);
    
    // 计算障碍物代价
    double calculateObstacleCost(const Trajectory& traj);
    
    // 计算目标代价
    double calculateGoalCost(const Trajectory& traj);
    
    // 计算路径代价
    double calculatePathCost(const Trajectory& traj);
    
    // 模拟轨迹
    Trajectory simulateTrajectory(double v, double w, double sim_time);
    
    // 检查轨迹是否与障碍物碰撞
    bool isTrajectoryValid(const Trajectory& traj);

    // ROS节点句柄
    ros::NodeHandle nh_;
    
    // 订阅者
    ros::Subscriber odom_sub_;
    ros::Subscriber scan_sub_;
    ros::Subscriber global_plan_sub_;
    
    // 发布者
    ros::Publisher cmd_vel_pub_;
    ros::Publisher trajectory_pub_;
    ros::Publisher selected_trajectory_pub_;
    
    // 当前状态
    geometry_msgs::PoseStamped current_pose_;
    geometry_msgs::Twist current_velocity_;
    std::vector<geometry_msgs::PoseStamped> global_plan_;
    nav_msgs::OccupancyGrid local_costmap_;
    
    // DWA参数
    double max_vel_x_;
    double min_vel_x_;
    double max_vel_y_;
    double min_vel_y_;
    double max_vel_trans_;
    double min_vel_trans_;
    double max_vel_rot_;
    double min_vel_rot_;
    double acc_lim_x_;
    double acc_lim_y_;
    double acc_lim_trans_;
    double acc_lim_rot_;
    double sim_time_;
    double sim_granularity_;
    
    // 代价权重
    double path_distance_bias_;
    double goal_distance_bias_;
    double occdist_scale_;
    
    // 其他参数
    double goal_tolerance_;
    double robot_radius_;
    double lookahead_distance_;
    
    // 轨迹评估参数
    double obstacle_weight_;
    double goal_weight_;
    double path_weight_;
};

} // namespace auto_navigation

#endif // DWA_PLANNER_HPP 